#include "main.h"

extern FLOAT_ANGLE Angle_offsum,Angle_off;   
extern FLOAT_XYZ ACC_OFFSET,ACC_SCALE,GYRO_OFFSET_RAW;
extern ADRC_TD_Def RC_TargetROL,RC_TargetPIT;
extern ADRC_TD_Def TD_TargetROLRATE;
extern ADRC_TD_Def Slide_TargetROL2d,Slide_TargetPIT2d;
extern RC_Dat RC_Data;
extern alt_control Alt_control;
extern alt_Status Alt_Status;
extern pos_Status Sys_Pos_Status; 
extern NDO High_NDO;
extern ADRC_TD_Def TD_TargetAlt;
extern Auto_Con Sys_Auto_Control;

enum Sys_Status sys_status;
enum Dev_Status dev_status;
uint8_t statusw; //状态定义
uint16_t time=0;
uint16_t i_time=0;


//串口1 TOF激光115200 普通超声波9600
//串口2 优像光流
//串口3 上位机
//串口6 103超声波  未使用

int main(void)
{
	SysTick_Configuration();  //滴答时钟配置
	delay_ms(200);	
	NVIV_Config();//配置中断优先级
	
	LEDInit();//LED指示灯

	UWB_Init();
	Usart3_Init(115200);//匿名上位机初始化
	//Usart3_Init(460800);//匿名上位机初始化
//  Usart6_Init(9600);  //超声波103
	delay_ms(200);	
//	OptFlowLC302_Init();  //光流初始化-串口2
	
	Usart1_Init(9600);   //TOF激光
	
	Init_GPS();         						//GPS初始化
	UART2_DMA_INIT();
	//Test_GPS();

	//Test_EGPS();
	
	IIC_Init();
	MPU6050_Init();    
	
	PPM_Init();//PPM解码初始化
	Pwm_Config(RCC_APB1Periph_TIM3,GPIO_AF_TIM3,TIM3);//电调初始化之前不能延时太多！！
	Motor_Pwm(0,0,0,0,TIM3); 
	delay_ms(300);
	
	//KS103_Init();
	
	statusw = AK8975_init();
    if(statusw)
        sys_status  = Error; 
	
//	HC_SR04_Init();//超声波模块初始化
	adrc_td_init(&RC_TargetROL,0.013,2000,0.015); 
	adrc_td_init(&RC_TargetPIT,0.013,2000,0.015);
	adrc_td_init(&TD_TargetROLRATE,0.005,100000,0.005);
	adrc_td_init(&TD_TargetAlt,0.01,400,0.018);	
	
	PidParameter_init();
	ADP_ADRC_ParaInit();
//	BaseState_NDObserver_Init(&High_NDO);
	
	SlideModePara_Init();//滑模参数初始化
	Rotor_Scale_Init(); //混控系数矩阵初始化

	sys_status = Normal;  
    RC_Data.LockFlag = 1;	
	Alt_Status.Trans = 1;	
	Alt_Status.Set_high_expect = 1;
	Sys_Pos_Status.Trans = 1;
	RC_Data.RC_SafeLimitT = 150.0f;  //45ms为安全极限时间
	
	    
    ACC_OFFSET.X = 0.0963693112;
    ACC_OFFSET.Y = -0.348487943;
    ACC_OFFSET.Z = -0.493184;
	
	ACC_SCALE.X = 0.997143626;
    ACC_SCALE.Y = 0.997909427;
    ACC_SCALE.Z = 0.989646435;
	
	Angle_off.rol = -0.932780206;
	Angle_off.pit = -1.97376728;
	
	GYRO_OFFSET_RAW.X = 0.025656458;
    GYRO_OFFSET_RAW.Y = -0.0516437255;
    GYRO_OFFSET_RAW.Z = -0.00487676822;

	TIM2_Init();     //
	
	IMU_Navigation_ACMP();
	TIM_Cmd(TIM2, ENABLE);   //先用定时器再开串口1 
	while(1)
	{
		ANO_DT_Data_Exchange(); //组成协议数据，当使用串口时将内部发送取消注释 	
		MYDMA_SendData(); //使用DMA发送数据至上位机，
		
//		if(Sys_Auto_Control.Enable==1)			
//		{	
//			//自动模式下要发命令时将数传周期调慢，以便接收外部的命令信息 用别的数传不必调慢
//			if(++time>=60000)
//			{
//				time=0; 
//				i_time++;
//				if(i_time>=3)
//				{i_time = 0;ANO_DT_Data_Exchange();}
//			}
//		}
//		else
//		{
//			time=0; i_time = 0;
//			ANO_DT_Data_Exchange(); 		
//		}
		
		


	}
}

